//
// Created by cch on 22-5-2.
//
#include "../include/LocalizationWrapper.h"

void LocalizationWrapper::imuCallBack(
    const sensor_msgs::ImuConstPtr &imu_msg_ptr) {
  IMUGPS::ImuDataPtr imuDataPtr = std::make_shared<IMUGPS::imuData>();
  // 获取时间戳
  imuDataPtr->timestamp = imu_msg_ptr->header.stamp.toSec();
  // 获取加速度
  imuDataPtr->acc << imu_msg_ptr->linear_acceleration.x,
      imu_msg_ptr->linear_acceleration.y, imu_msg_ptr->linear_acceleration.z;
  // 获取角速度
  imuDataPtr->gyro << imu_msg_ptr->angular_velocity.x,
      imu_msg_ptr->angular_velocity.y, imu_msg_ptr->angular_velocity.z;
  IMUGPS::State fusedState;
  bool ok = imu_gps_fusion_ptr_->ProcessImuData(imuDataPtr, &fusedState);
  if (!ok) {
    return;
  }
  ConvertStateToRosTopic(&fusedState);
  state_pub_.publish(ros_path_);
  LogState(fusedState);
}

void LocalizationWrapper::GpsPositionCallBack(
    const sensor_msgs::NavSatFixConstPtr &gps_msg_ptr) {
  assert(gps_msg_ptr->status.status != 2);
  if (gps_msg_ptr->status.status != 2) {
    ROS_INFO("bad message");
    return;
  }
  IMUGPS::GpsDataPtr gpsDataPtr = std::make_shared<IMUGPS::gpsData>();
  gpsDataPtr->timestamp = gps_msg_ptr->header.stamp.toSec();
  gpsDataPtr->lla << gps_msg_ptr->longitude, gps_msg_ptr->latitude,
      gps_msg_ptr->altitude;

  gpsDataPtr->cov = Eigen::Map<const Eigen::Matrix3d>(
      gps_msg_ptr->position_covariance.data());

  imu_gps_fusion_ptr_->ProcessGpsData(gpsDataPtr);
  LogGps(gpsDataPtr);
}

void LocalizationWrapper::LogState(const IMUGPS::State &state) {
  const Eigen::Quaterniond G_Q_I(state.G_R_I);
  file_state_ << std::fixed << std::setprecision(15) << state.timestamp << ","
              << state.lla[0] << "," << state.lla[1] << "," << state.lla[2]
              << "," << state.G_P_I[0] << "," << state.G_P_I[1] << ","
              << state.G_P_I[2] << "," << state.G_V_I[0] << ","
              << state.G_V_I[1] << "," << state.G_V_I[2] << "," << G_Q_I.x()
              << "," << G_Q_I.y() << "," << G_Q_I.z() << "," << G_Q_I.w() << ","
              << state.acc_bias[0] << "," << state.acc_bias[1] << ","
              << state.acc_bias[2] << "," << state.gyro_bias[0] << ","
              << state.gyro_bias[1] << "," << state.gyro_bias[2] << "\n";
}

void LocalizationWrapper::LogGps(const IMUGPS::GpsDataPtr &gps_data_ptr) {
  file_gps_ << std::fixed << std::setprecision(15) << gps_data_ptr->timestamp
            << "," << gps_data_ptr->lla[0] << "," << gps_data_ptr->lla[1] << ","
            << gps_data_ptr->lla[2] << "\n";
}

void LocalizationWrapper::ConvertStateToRosTopic(const IMUGPS::State *state) {

  ros_path_.header.frame_id = "world";
  ros_path_.header.stamp = ros::Time::now();

  geometry_msgs::PoseStamped pose;
  pose.header = ros_path_.header;

  // 获取位置
  pose.pose.position.x = state->G_P_I[0];
  pose.pose.position.y = state->G_P_I[1];
  pose.pose.position.z = state->G_P_I[2];

  // 获取姿态
  const Eigen::Quaterniond G_Q_I(state->G_R_I);
  pose.pose.orientation.x = G_Q_I.x();
  pose.pose.orientation.y = G_Q_I.y();
  pose.pose.orientation.z = G_Q_I.z();
  pose.pose.orientation.w = G_Q_I.w();

  ros_path_.poses.push_back(pose);
}